{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 190,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "import math\n",
    "plt.style.use({'figure.figsize':(10, 10)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 191,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#constant\n",
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_Rate=0.000001#he dacaying rate of the Epsilon, the range of the epsilon is 0.01-1, initially it is 1.\n",
    "Action_times=0 #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Velocity_tripod=0.289*40\n",
    "Robot_R=40\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 192,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style>\n",
       "    .dataframe thead tr:only-child th {\n",
       "        text-align: right;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: left;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>Up</th>\n",
       "      <th>Left_45D</th>\n",
       "      <th>Right_45D</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>L60D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D30/60</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "            Up  Left_45D  Right_45D\n",
       "L60D0/30   0.0       0.0        0.0\n",
       "L180D0/30  0.0       0.0        0.0\n",
       "L250D0/30  0.0       0.0        0.0\n",
       "L300D0/30  0.0       0.0        0.0\n",
       "L60D30/60  0.0       0.0        0.0"
      ]
     },
     "execution_count": 192,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "States2=np.array(['L60D0/30','L180D0/30','L250D0/30','L300D0/30',\n",
    "                'L60D30/60','L180D30/60','L250D30/60','L300D30/60',\n",
    "                 'L60D60/90','L180D60/90','L250D60/90','L300D60/90',\n",
    "                'L60D90/120','L180D90/120','L250D90/120','L300D90/120',\n",
    "                'L60D120/150','L180D120/150','L250D120/150','L300D120/150',\n",
    "                'L60D150/180','L180D150/180','L250D150/180','L300D150/180',\n",
    "                'L60D180/210','L180D180/210','L250D180/210','L300D180/210',\n",
    "                'L60D210/240','L180D210/240','L250D210/240','L300D210/240',\n",
    "                'L60D240/270','L180D240/270','L250D240/270','L300D240/270',\n",
    "                'L60D270/300','L180D270/300','L250D270/300','L300D270/300',\n",
    "                'L60D300/330','L180D300/330','L250D300/330','L300D300/330',\n",
    "                 'L60D330/360','L180D330/360','L250D330/360','L300D330/360',])\n",
    "Actions2=np.array(['Up','Left_45D','Right_45D'])\n",
    "Actions_len2=len(Actions2)\n",
    "States_len2=len(States2)\n",
    "Q_Table_Real2=np.zeros((States_len2,Actions_len2))\n",
    "Q_Table_Real2=pd.DataFrame(Q_Table_Real2,columns=Actions2,index=States2)\n",
    "Q_Table_Real2.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 193,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Set_destination(x_,y_):\n",
    "    return x_,y_\n",
    "des_x,des_y=Set_destination(900,900)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 194,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAlsAAAJCCAYAAAD3HAIiAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nSUVORK5CYII=\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x280232f4ba8>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#Draw_map2 funcion is used to train the first Q-Table2, which allows it to arrive to the destination\n",
    "im = Image.new(\"I\", size=(1000,1000),color=0) #Color means the degree of Gray scale, 255 means white, 0 means black\n",
    "draw = ImageDraw.Draw(im,mode='I') #mode=\"I\" means the made degree of Gray scale\n",
    "#Draw the black frame around the rectangle\n",
    "draw.rectangle((40,40,960,960),255,255)  #the last elements means the color of the inner graph and that of the outer frame\n",
    "def Draw_map2_with_dis(x_,y_):\n",
    "    draw.ellipse((x_-10,y_-10,x_+10,y_+10),2, 2) #the color of destination is 2\n",
    "Draw_map2_with_dis(des_x,des_y)\n",
    "plt.imshow(im)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 195,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Initial_Q_Table2(Actions_len2_,States_len2_):\n",
    "    Q_Table2_=np.zeros((States_len2_,Actions_len2_))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-Table2!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_Table2_\n",
    "# Q_Table2=Initial_Q_Table2(Actions_len2,States_len2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 196,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(105.39554925399509, 'right', 20.242529486207992)"
      ]
     },
     "execution_count": 196,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def Robot_desti_(desti_x_,desti_y_,robot_x_,robot_y_,robot_a):\n",
    "    #Left-negative,180-360 |||| Right-positive,0-180\n",
    "    delta_x=desti_x_-robot_x_\n",
    "    delta_y=desti_y_-robot_y_\n",
    "    distance=np.sqrt(delta_x*delta_x+delta_y*delta_y)\n",
    "#     print(delta_x)\n",
    "#     print(delta_y)\n",
    "#     print((np.arctan(delta_y/delta_x))*180/np.pi)\n",
    "    direct_angle=(np.arctan(delta_y/delta_x))*180/np.pi\n",
    "    if delta_x<0:\n",
    "        direct_angle=direct_angle+180\n",
    "    true_angle=(robot_a-direct_angle)%360\n",
    "#     print(true_angle)\n",
    "    if true_angle>180:\n",
    "        return 360-true_angle,'right',distance\n",
    "    else:\n",
    "        return true_angle,'left',distance\n",
    "(Robot_desti_(0,10,17.6,0,45))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 197,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#Is_Crash function, in the second map, is used to judge if the robot collides with the black frame\n",
    "def Is_Crash(Current_x_,Current_y_):\n",
    "    Crash_=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,50,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x_=Current_x_+i*np.cos(j/180*np.pi)\n",
    "            y_=Current_y_+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x_,y_)))==0:\n",
    "                Crash_=True\n",
    "                break\n",
    "        if Crash_==True:\n",
    "                break\n",
    "    return Crash_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 198,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "False\n"
     ]
    }
   ],
   "source": [
    "def Is_arrive(Current_x_,Current_y_):\n",
    "    Arrive_=False\n",
    "    Crash_=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,40,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x_=Current_x_+i*np.cos(j/180*np.pi)\n",
    "            y_=Current_y_+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x_,y_)))==0:\n",
    "                Crash_=True\n",
    "                break\n",
    "            if (im.getpixel((x_,y_)))==2:\n",
    "                Arrive_=True\n",
    "                break\n",
    "        if Arrive_==True or Crash_==True:\n",
    "                break\n",
    "    return Arrive_\n",
    "print(Is_arrive(500,500))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 199,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#Random_start function, in the second map, is used to set the beginning point in the white map.\n",
    "def Random_start():\n",
    "    Angle_=0\n",
    "    x_,y_=np.random.random(2)*1000\n",
    "    while(Is_Crash(x_,y_)==True or Is_arrive(x_,y_)==True):\n",
    "#         print('Boom')\n",
    "        x_,y_=np.random.random(2)*1000\n",
    "    return x_,y_,Angle_\n",
    "#print(Random_start())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 200,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'L300D330/360'"
      ]
     },
     "execution_count": 200,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def Output_state_index(true_angle,left_right,distance_):\n",
    "    if 0<=distance_<60:\n",
    "        dis_level=0\n",
    "    elif 60<=distance_<180:\n",
    "        dis_level=1\n",
    "    elif 180<=distance_<250:\n",
    "        dis_level=2\n",
    "    else:\n",
    "        dis_level=3\n",
    "    \n",
    "    angle_level=true_angle//30\n",
    "    \n",
    "    if left_right=='right':\n",
    "        le_ri=1\n",
    "        State_number=int(dis_level+24*le_ri+(5-angle_level)*4)\n",
    "    elif left_right=='left':\n",
    "        le_ri=0\n",
    "        angle_level=true_angle//30\n",
    "        State_number=int(dis_level+24*le_ri+angle_level*4)\n",
    "\n",
    "    return State_number,dis_level,angle_level\n",
    "State_Number,A,B=Output_state_index(20,'right',299) #180 belongs to 180-210,''right' does not have 180\n",
    "States2[State_Number]\n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 201,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Choose_action2(Q_Table_,Current_state_,Action_times_):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times_)\n",
    "    State_action_=Q_Table_[Current_state_,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action_==[0])):\n",
    "        Action_next=np.random.randint(Actions_len2)\n",
    "    else:\n",
    "        Action_next=np.argmax(State_action_)\n",
    "    return Action_next\n",
    "# print(Choose_action2(Q_Table2,20,0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 202,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Next_state_F2(Robot_x_,Robot_y_,Robot_a_,Desti_x_,Desti_y_,Action_):\n",
    "    Arrive_=False\n",
    "    Crash_=False\n",
    "    if Action_==0:\n",
    "        Ro_next_x=Robot_x_+Velocity_tripod*np.cos(Robot_a_/180*np.pi)\n",
    "        Ro_next_y=Robot_y_+Velocity_tripod*np.sin(Robot_a_/180*np.pi)\n",
    "        if Is_Crash(Ro_next_x,Ro_next_y)==True:\n",
    "#             print(\"Boom!\")\n",
    "            Crash_=True\n",
    "        if Is_arrive(Ro_next_x,Ro_next_y)==True:\n",
    "            Arrive_=True\n",
    "    elif Action_==1:\n",
    "        Robot_a_=Robot_a_-45\n",
    "        Ro_next_x=Robot_x_\n",
    "        Ro_next_y=Robot_y_\n",
    "    elif Action_==2:\n",
    "        Robot_a_=Robot_a_+45\n",
    "        Ro_next_x=Robot_x_\n",
    "        Ro_next_y=Robot_y_\n",
    "    True_angle_,Le_ri_,Distance_=Robot_desti_(Desti_x_,Desti_y_,Robot_x_,Robot_y_,Robot_a_)\n",
    "    State_Number_,Distance_level,Angle_level=Output_state_index(True_angle_,Le_ri_,Distance_)\n",
    "    if Arrive_==False:\n",
    "        R=Distance_level*-0.5-Angle_level\n",
    "    elif Arrive_==True:\n",
    "        R=50\n",
    "    return Ro_next_x,Ro_next_y,Robot_a_,R,State_Number_,Arrive_,Crash_\n",
    "#Next_state_F2(600,600,50,1800,1800,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 203,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Plot_Move(Vec_x_,Vec_y_,Destination_x_,Destination_y_):\n",
    "    im1=Image.new(\"I\", size=(1000,1000),color=0)\n",
    "    draw1 = ImageDraw.Draw(im1,mode='I')\n",
    "    draw1.rectangle((40,40,960,960),255,255)\n",
    "    draw1.ellipse((Destination_x_-10,Destination_y_-10,Destination_x_+10,Destination_y_+10),2, 2)\n",
    "    for i,j in zip(Vec_x_,Vec_y_):\n",
    "        draw1.ellipse((i-40,j-40, i+40,j+40),150,150) \n",
    "    plt.imshow(im1)\n",
    "    plt.show()\n",
    "    del draw1\n",
    "    del im1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 204,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "***********************************************************\n",
      "Succeed to initialize Q-Table2!\n",
      "***********************************************************\n"
     ]
    }
   ],
   "source": [
    "Q_Table2=Initial_Q_Table2(Actions_len2,States_len2)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Action_times=0\n",
    "global Epoche_action_interval"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 205,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "**********************************\n",
      "last_x=885.847847, last_y=857.032597,last_a=90.000000\n",
      "x=885.847847 y=868.592597 True_angle=24.256292 Des_Ro_dis=34.448634 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=65.743708 Robot_a=90\n",
      "0.951530688722\n",
      "50198\n",
      "**********************************\n",
      "last_x=875.125941, last_y=861.709421,last_a=45.000000\n",
      "x=883.300095 y=869.883576 True_angle=15.991126 Des_Ro_dis=34.436693 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=60.991126 Robot_a=45\n",
      "0.949245546721\n",
      "52628\n",
      "**********************************\n",
      "last_x=854.150158, last_y=896.189657,last_a=45.000000\n",
      "x=862.324312 y=904.363811 True_angle=51.606881 Des_Ro_dis=37.927566 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-6.606881 Robot_a=45\n",
      "0.949009825673\n",
      "52879\n",
      "**********************************\n",
      "last_x=874.015864, last_y=860.591107,last_a=90.000000\n",
      "x=874.015864 y=872.151107 True_angle=43.016088 Des_Ro_dis=38.088531 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=46.983912 Robot_a=90\n",
      "0.948775102558\n",
      "53129\n",
      "**********************************\n",
      "last_x=858.934621, last_y=889.934475,last_a=45.000000\n",
      "x=867.108775 y=898.108629 True_angle=41.708899 Des_Ro_dis=32.945560 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=3.291101 Robot_a=45\n",
      "0.948487881324\n",
      "53435\n",
      "**********************************\n",
      "last_x=891.186825, last_y=848.929752,last_a=90.000000\n",
      "x=891.186825 y=860.489752 True_angle=12.574568 Des_Ro_dis=40.481252 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=77.425432 Robot_a=90\n",
      "0.945979845002\n",
      "56111\n",
      "**********************************\n",
      "last_x=852.107310, last_y=884.113832,last_a=45.000000\n",
      "x=860.281465 y=892.287987 True_angle=34.011799 Des_Ro_dis=40.460316 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=10.988201 Robot_a=45\n",
      "0.940498015289\n",
      "61985\n",
      "**********************************\n",
      "last_x=868.228643, last_y=860.993588,last_a=45.000000\n",
      "x=876.402798 y=869.167743 True_angle=7.571675 Des_Ro_dis=38.825971 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=52.571675 Robot_a=45\n",
      "0.936822383019\n",
      "65943\n",
      "**********************************\n",
      "last_x=847.460592, last_y=898.598340,last_a=0.000000\n",
      "x=859.020592 y=898.598340 True_angle=1.958982 Des_Ro_dis=41.003372 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=1.958982 Robot_a=0\n",
      "0.932030167939\n",
      "71127\n",
      "**********************************\n",
      "last_x=852.873902, last_y=888.416763,last_a=45.000000\n",
      "x=861.048056 y=896.590918 True_angle=39.998206 Des_Ro_dis=39.100841 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=5.001794 Robot_a=45\n",
      "0.931492781015\n",
      "71710\n",
      "**********************************\n",
      "last_x=863.565681, last_y=870.728469,last_a=45.000000\n",
      "x=871.739836 y=878.902623 True_angle=8.257145 Des_Ro_dis=35.266644 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=36.742855 Robot_a=45\n",
      "0.930350838117\n",
      "72950\n",
      "**********************************\n",
      "last_x=859.299933, last_y=887.006599,last_a=0.000000\n",
      "x=870.859933 y=887.006599 True_angle=24.031837 Des_Ro_dis=31.905673 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=24.031837 Robot_a=0\n",
      "0.928356760508\n",
      "75119\n",
      "**********************************\n",
      "last_x=878.035625, last_y=860.083565,last_a=45.000000\n",
      "x=886.209779 y=868.257720 True_angle=21.517787 Des_Ro_dis=34.608417 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=66.517787 Robot_a=45\n",
      "0.927499415736\n",
      "76053\n",
      "**********************************\n",
      "last_x=876.930110, last_y=854.203046,last_a=45.000000\n",
      "x=885.104264 y=862.377200 True_angle=23.400243 Des_Ro_dis=40.464281 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=68.400243 Robot_a=45\n",
      "0.922555534316\n",
      "81456\n",
      "**********************************\n",
      "last_x=898.086111, last_y=849.823461,last_a=90.000000\n",
      "x=898.086111 y=861.383461 True_angle=2.837337 Des_Ro_dis=38.663938 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=87.162663 Robot_a=90\n",
      "0.921256980025\n",
      "82880\n",
      "**********************************\n",
      "last_x=869.969216, last_y=858.751165,last_a=45.000000\n",
      "x=878.143371 y=866.925319 True_angle=11.542210 Des_Ro_dis=39.644000 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=56.542210 Robot_a=45\n",
      "0.920480009246\n",
      "83733\n",
      "**********************************\n",
      "last_x=884.959943, last_y=855.219981,last_a=45.000000\n",
      "x=893.134097 y=863.394135 True_angle=34.376862 Des_Ro_dis=37.244194 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=79.376862 Robot_a=45\n",
      "0.91810403055\n",
      "86346\n",
      "**********************************\n",
      "last_x=849.426066, last_y=885.761454,last_a=45.000000\n",
      "x=857.600220 y=893.935609 True_angle=36.860257 Des_Ro_dis=42.831276 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=8.139743 Robot_a=45\n",
      "0.91554950124\n",
      "89163\n",
      "**********************************\n",
      "last_x=892.190593, last_y=854.886303,last_a=45.000000\n",
      "x=900.364747 y=863.060458 True_angle=45.565730 Des_Ro_dis=36.941343 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-89.434270 Robot_a=45\n",
      "0.914665211423\n",
      "90140\n",
      "**********************************\n",
      "last_x=850.029000, last_y=885.957844,last_a=0.000000\n",
      "x=861.589000 y=885.957844 True_angle=20.081220 Des_Ro_dis=40.897275 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=20.081220 Robot_a=0\n",
      "0.913231549592\n",
      "91726\n",
      "**********************************\n",
      "last_x=858.923022, last_y=876.513533,last_a=45.000000\n",
      "x=867.097176 y=884.687688 True_angle=20.043679 Des_Ro_dis=36.291359 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=24.956321 Robot_a=45\n",
      "0.912346816263\n",
      "92706\n",
      "**********************************\n",
      "last_x=894.182220, last_y=848.516314,last_a=90.000000\n",
      "x=894.182220 y=860.076314 True_angle=8.290928 Des_Ro_dis=40.345350 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=81.709072 Robot_a=90\n",
      "0.912119453515\n",
      "92958\n",
      "**********************************\n",
      "last_x=880.191030, last_y=851.917297,last_a=90.000000\n",
      "x=880.191030 y=863.477297 True_angle=28.474260 Des_Ro_dis=41.548804 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=61.525740 Robot_a=90\n",
      "0.910235992759\n",
      "95048\n",
      "**********************************\n",
      "last_x=887.886771, last_y=858.457462,last_a=0.000000\n",
      "x=899.446771 y=858.457462 True_angle=89.237027 Des_Ro_dis=41.546222 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=89.237027 Robot_a=0\n",
      "0.909360489139\n",
      "96021\n",
      "**********************************\n",
      "last_x=869.065996, last_y=859.862037,last_a=45.000000\n",
      "x=877.240151 y=868.036191 True_angle=9.547210 Des_Ro_dis=39.238958 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=54.547210 Robot_a=45\n",
      "0.904283593123\n",
      "101682\n",
      "**********************************\n",
      "last_x=888.021134, last_y=858.630026,last_a=0.000000\n",
      "x=899.581134 y=858.630026 True_angle=89.419907 Des_Ro_dis=41.372095 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=89.419907 Robot_a=0\n",
      "0.903662281956\n",
      "102377\n",
      "**********************************\n",
      "last_x=854.259622, last_y=890.230804,last_a=45.000000\n",
      "x=862.433777 y=898.404958 True_angle=42.568712 Des_Ro_dis=37.600071 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=2.431288 Robot_a=45\n",
      "0.90319144601\n",
      "102904\n",
      "**********************************\n",
      "last_x=915.113466, last_y=849.717775,last_a=135.000000\n",
      "x=906.939311 y=857.891930 True_angle=35.641901 Des_Ro_dis=42.676031 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-80.641901 Robot_a=135\n",
      "0.902195200365\n",
      "104020\n",
      "**********************************\n",
      "last_x=851.924695, last_y=889.795250,last_a=0.000000\n",
      "x=863.484695 y=889.795250 True_angle=15.613818 Des_Ro_dis=37.914435 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=15.613818 Robot_a=0\n",
      "0.900471362336\n",
      "105954\n",
      "**********************************\n",
      "last_x=856.980520, last_y=891.988155,last_a=45.000000\n",
      "x=865.154674 y=900.162309 True_angle=45.266881 Des_Ro_dis=34.845704 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-0.266881 Robot_a=45\n",
      "0.900401908278\n",
      "106032\n",
      "**********************************\n",
      "last_x=860.309814, last_y=877.801659,last_a=45.000000\n",
      "x=868.483968 y=885.975814 True_angle=21.011616 Des_Ro_dis=34.495479 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=23.988384 Robot_a=45\n",
      "0.898654875079\n",
      "107996\n",
      "**********************************\n",
      "last_x=870.753512, last_y=857.293216,last_a=45.000000\n",
      "x=878.927666 y=865.467370 True_angle=13.607826 Des_Ro_dis=40.454243 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=58.607826 Robot_a=45\n",
      "0.897360160347\n",
      "109454\n",
      "**********************************\n",
      "last_x=866.794942, last_y=862.494126,last_a=45.000000\n",
      "x=874.969096 y=870.668280 True_angle=4.523447 Des_Ro_dis=38.560290 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=49.523447 Robot_a=45\n",
      "0.895501315771\n",
      "111551\n",
      "**********************************\n",
      "last_x=885.622586, last_y=855.089854,last_a=45.000000\n",
      "x=893.796741 y=863.264008 True_angle=35.415420 Des_Ro_dis=37.256054 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=80.415420 Robot_a=45\n",
      "0.894513647524\n",
      "112667\n",
      "**********************************\n",
      "last_x=883.054063, last_y=853.046978,last_a=90.000000\n",
      "x=883.054063 y=864.606978 True_angle=25.584781 Des_Ro_dis=39.240678 Action=0 S_N=0\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "L60D0/30\n",
      "Des_Robot_a=64.415219 Robot_a=90\n",
      "0.893343326378\n",
      "113991\n",
      "**********************************\n",
      "last_x=899.554006, last_y=847.738620,last_a=90.000000\n",
      "x=899.554006 y=859.298620 True_angle=0.627806 Des_Ro_dis=40.703823 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=89.372194 Robot_a=90\n",
      "0.893075713895\n",
      "114294\n",
      "**********************************\n",
      "last_x=854.656932, last_y=882.486054,last_a=0.000000\n",
      "x=866.216932 y=882.486054 True_angle=27.403298 Des_Ro_dis=38.053041 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=27.403298 Robot_a=0\n",
      "0.892620164369\n",
      "114810\n",
      "**********************************\n",
      "last_x=861.646293, last_y=873.482709,last_a=0.000000\n",
      "x=873.206293 y=873.482709 True_angle=44.702926 Des_Ro_dis=37.697075 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=44.702926 Robot_a=0\n",
      "0.892246012745\n",
      "115234\n",
      "**********************************\n",
      "last_x=885.600017, last_y=855.159873,last_a=45.000000\n",
      "x=893.774172 y=863.334028 True_angle=35.363174 Des_Ro_dis=37.190785 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=80.363174 Robot_a=45\n",
      "0.891895830593\n",
      "115631\n",
      "**********************************\n",
      "last_x=850.055419, last_y=883.920940,last_a=45.000000\n",
      "x=858.229573 y=892.095095 True_angle=34.283709 Des_Ro_dis=42.511835 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=10.716291 Robot_a=45\n",
      "0.891242587839\n",
      "116372\n",
      "**********************************\n",
      "last_x=870.592836, last_y=862.546866,last_a=90.000000\n",
      "x=870.592836 y=874.106866 True_angle=48.635943 Des_Ro_dis=39.182083 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=41.364057 Robot_a=90\n",
      "0.883971654431\n",
      "124657\n",
      "**********************************\n",
      "last_x=869.928722, last_y=858.250099,last_a=45.000000\n",
      "x=878.102877 y=866.424254 True_angle=11.888781 Des_Ro_dis=40.085094 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=56.888781 Robot_a=45\n",
      "0.88297764039\n",
      "125795\n",
      "**********************************\n",
      "last_x=855.580111, last_y=873.467753,last_a=0.000000\n",
      "x=867.140111 y=873.467753 True_angle=38.918644 Des_Ro_dis=42.234257 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=38.918644 Robot_a=0\n",
      "0.882174870291\n",
      "126715\n",
      "**********************************\n",
      "last_x=875.683395, last_y=854.807747,last_a=90.000000\n",
      "x=875.683395 y=866.367747 True_angle=35.867473 Des_Ro_dis=41.502117 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=54.132527 Robot_a=90\n",
      "0.880534980806\n",
      "128597\n",
      "**********************************\n",
      "last_x=906.496113, last_y=849.958773,last_a=90.000000\n",
      "x=906.496113 y=861.518773 True_angle=9.581905 Des_Ro_dis=39.025688 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-80.418095 Robot_a=90\n",
      "0.879737935865\n",
      "129513\n",
      "**********************************\n",
      "last_x=887.628658, last_y=854.858846,last_a=45.000000\n",
      "x=895.802812 y=863.033001 True_angle=38.522446 Des_Ro_dis=37.204508 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=83.522446 Robot_a=45\n",
      "0.878646230752\n",
      "130769\n",
      "**********************************\n",
      "last_x=876.055163, last_y=858.775925,last_a=90.000000\n",
      "x=876.055163 y=870.335925 True_angle=38.910463 Des_Ro_dis=38.122336 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=51.089537 Robot_a=90\n",
      "0.878338784408\n",
      "131123\n",
      "**********************************\n",
      "last_x=855.496935, last_y=885.914163,last_a=90.000000\n",
      "x=855.496935 y=897.474163 True_angle=86.751579 Des_Ro_dis=44.574686 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=3.248421 Robot_a=90\n",
      "0.877984574448\n",
      "131531\n",
      "**********************************\n",
      "last_x=852.547470, last_y=915.454969,last_a=315.000000\n",
      "x=860.721624 y=907.280814 True_angle=34.498593 Des_Ro_dis=39.947479 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-10.501407 Robot_a=315\n",
      "0.877419700376\n",
      "132182\n",
      "**********************************\n",
      "last_x=862.011044, last_y=872.292179,last_a=0.000000\n",
      "x=873.571044 y=872.292179 True_angle=46.353238 Des_Ro_dis=38.291162 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=46.353238 Robot_a=0\n",
      "0.877408423993\n",
      "132195\n",
      "**********************************\n",
      "last_x=857.677284, last_y=884.512976,last_a=45.000000\n",
      "x=865.851438 y=892.687130 True_angle=32.912749 Des_Ro_dis=34.922805 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=12.087251 Robot_a=45\n",
      "0.876834389643\n",
      "132857\n",
      "**********************************\n",
      "last_x=853.242453, last_y=877.984054,last_a=45.000000\n",
      "x=861.416607 y=886.158208 True_angle=25.264552 Des_Ro_dis=40.991138 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=19.735448 Robot_a=45\n",
      "0.876465196837\n",
      "133283\n",
      "**********************************\n",
      "last_x=856.303600, last_y=887.987716,last_a=45.000000\n",
      "x=864.477754 y=896.161870 True_angle=38.833193 Des_Ro_dis=35.728996 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=6.166807 Robot_a=45\n",
      "0.87563119249\n",
      "134246\n",
      "**********************************\n",
      "last_x=866.388225, last_y=863.468421,last_a=45.000000\n",
      "x=874.562380 y=871.642575 True_angle=3.106764 Des_Ro_dis=38.094830 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=48.106764 Robot_a=45\n",
      "0.87533519724\n",
      "134588\n",
      "**********************************\n",
      "last_x=854.875575, last_y=900.406518,last_a=0.000000\n",
      "x=866.435575 y=900.406518 True_angle=0.693907 Des_Ro_dis=33.566887 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-0.693907 Robot_a=0\n",
      "0.875054008996\n",
      "134913\n",
      "**********************************\n",
      "last_x=862.766949, last_y=867.753190,last_a=0.000000\n",
      "x=874.326949 y=867.753190 True_angle=51.475218 Des_Ro_dis=41.218470 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=51.475218 Robot_a=0\n",
      "0.874385580628\n",
      "135686\n",
      "**********************************\n",
      "last_x=875.616863, last_y=855.473435,last_a=90.000000\n",
      "x=875.616863 y=867.033435 True_angle=36.487826 Des_Ro_dis=41.004046 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=53.512174 Robot_a=90\n",
      "0.87088759406\n",
      "139741\n",
      "**********************************\n",
      "last_x=851.111489, last_y=881.052470,last_a=0.000000\n",
      "x=862.671489 y=881.052470 True_angle=26.911834 Des_Ro_dis=41.861995 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=26.911834 Robot_a=0\n",
      "0.870213782905\n",
      "140524\n",
      "**********************************\n",
      "last_x=880.648549, last_y=859.574853,last_a=90.000000\n",
      "x=880.648549 y=871.134853 True_angle=33.838251 Des_Ro_dis=34.751624 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=56.161749 Robot_a=90\n",
      "0.869635053793\n",
      "141197\n",
      "**********************************\n",
      "last_x=852.291925, last_y=871.724189,last_a=45.000000\n",
      "x=860.466080 y=879.898343 True_angle=18.048209 Des_Ro_dis=44.350958 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=26.951791 Robot_a=45\n",
      "0.86846674475\n",
      "142557\n",
      "**********************************\n",
      "last_x=862.765415, last_y=870.102646,last_a=0.000000\n",
      "x=874.325415 y=870.102646 True_angle=49.345409 Des_Ro_dis=39.408578 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=49.345409 Robot_a=0\n",
      "0.867784534903\n",
      "143352\n",
      "**********************************\n",
      "last_x=855.390812, last_y=896.000902,last_a=45.000000\n",
      "x=863.564967 y=904.175056 True_angle=51.536957 Des_Ro_dis=36.673461 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-6.536957 Robot_a=45\n",
      "0.867444062031\n",
      "143749\n",
      "**********************************\n",
      "last_x=870.644050, last_y=871.657768,last_a=0.000000\n",
      "x=882.204050 y=871.657768 True_angle=57.875488 Des_Ro_dis=33.466071 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=57.875488 Robot_a=0\n",
      "0.866249617729\n",
      "145143\n",
      "**********************************\n",
      "last_x=870.422843, last_y=871.471184,last_a=0.000000\n",
      "x=881.982843 y=871.471184 True_angle=57.725852 Des_Ro_dis=33.741833 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=57.725852 Robot_a=0\n",
      "0.864630270399\n",
      "147036\n",
      "**********************************\n",
      "last_x=854.623111, last_y=882.651180,last_a=45.000000\n",
      "x=862.797265 y=890.825335 True_angle=31.146565 Des_Ro_dis=38.317332 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=13.853435 Robot_a=45\n",
      "0.864570448373\n",
      "147106\n",
      "**********************************\n",
      "last_x=890.070621, last_y=859.469926,last_a=90.000000\n",
      "x=890.070621 y=871.029926 True_angle=18.918949 Des_Ro_dis=30.624464 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=71.081051 Robot_a=90\n",
      "0.864137290971\n",
      "147613\n",
      "**********************************\n",
      "last_x=854.567096, last_y=869.110014,last_a=45.000000\n",
      "x=862.741250 y=877.284169 True_angle=13.630267 Des_Ro_dis=43.637409 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=31.369733 Robot_a=45\n",
      "0.863502902668\n",
      "148356\n",
      "**********************************\n",
      "last_x=878.282605, last_y=860.434047,last_a=45.000000\n",
      "x=886.456760 y=868.608201 True_angle=21.663374 Des_Ro_dis=34.188659 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=66.663374 Robot_a=45\n",
      "0.863056637359\n",
      "148879\n",
      "**********************************\n",
      "last_x=903.256249, last_y=844.883065,last_a=90.000000\n",
      "x=903.256249 y=856.443065 True_angle=4.275391 Des_Ro_dis=43.678482 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-85.724609 Robot_a=90\n",
      "0.862221903579\n",
      "149858\n",
      "**********************************\n",
      "last_x=890.600950, last_y=858.496424,last_a=0.000000\n",
      "x=902.160950 y=858.496424 True_angle=92.980505 Des_Ro_dis=41.559795 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=-87.019495 Robot_a=0\n",
      "0.86096324998\n",
      "151336\n",
      "**********************************\n",
      "last_x=853.868909, last_y=907.306158,last_a=0.000000\n",
      "x=865.428909 y=907.306158 True_angle=11.933140 Des_Ro_dis=35.334690 Action=0 S_N=0\n",
      "L60D0/30\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Des_Robot_a=-11.933140 Robot_a=0\n",
      "0.857859504261\n",
      "154990\n",
      "**********************************\n",
      "last_x=875.391918, last_y=854.089180,last_a=45.000000\n",
      "x=883.566072 y=862.263335 True_angle=21.467372 Des_Ro_dis=41.159809 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=66.467372 Robot_a=45\n",
      "0.857372972554\n",
      "155564\n",
      "**********************************\n",
      "last_x=850.799479, last_y=884.534866,last_a=45.000000\n",
      "x=858.973633 y=892.709021 True_angle=34.922918 Des_Ro_dis=41.669187 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=10.077082 Robot_a=45\n",
      "0.857013762571\n",
      "155988\n",
      "**********************************\n",
      "last_x=874.463598, last_y=853.690475,last_a=45.000000\n",
      "x=882.637753 y=861.864630 True_angle=20.521205 Des_Ro_dis=41.901720 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=65.521205 Robot_a=45\n",
      "0.856184095687\n",
      "156968\n",
      "**********************************\n",
      "last_x=872.572882, last_y=866.635293,last_a=0.000000\n",
      "x=884.132882 y=866.635293 True_angle=64.565831 Des_Ro_dis=36.945488 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=64.565831 Robot_a=0\n",
      "0.855817777288\n",
      "157401\n",
      "**********************************\n",
      "last_x=850.746103, last_y=896.549364,last_a=0.000000\n",
      "x=862.306103 y=896.549364 True_angle=5.230485 Des_Ro_dis=37.851509 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=5.230485 Robot_a=0\n",
      "0.855314665401\n",
      "157996\n",
      "**********************************\n",
      "last_x=862.322260, last_y=873.358633,last_a=45.000000\n",
      "x=870.496415 y=881.532788 True_angle=12.956269 Des_Ro_dis=34.806601 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=32.043731 Robot_a=45\n",
      "0.854499330211\n",
      "158961\n",
      "**********************************\n",
      "last_x=887.902701, last_y=854.551499,last_a=45.000000\n",
      "x=896.076855 y=862.725654 True_angle=38.991709 Des_Ro_dis=37.480234 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=83.991709 Robot_a=45\n",
      "0.853474730082\n",
      "160175\n",
      "**********************************\n",
      "last_x=869.072428, last_y=868.094768,last_a=45.000000\n",
      "x=877.246582 y=876.268923 True_angle=1.204864 Des_Ro_dis=32.876770 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=46.204864 Robot_a=45\n",
      "0.852789263807\n",
      "160988\n",
      "**********************************\n",
      "last_x=857.938462, last_y=881.952170,last_a=0.000000\n",
      "x=869.498462 y=881.952170 True_angle=30.612901 Des_Ro_dis=35.441049 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=30.612901 Robot_a=0\n",
      "0.852465694858\n",
      "161372\n",
      "**********************************\n",
      "last_x=866.902853, last_y=871.443589,last_a=45.000000\n",
      "x=875.077007 y=879.617743 True_angle=5.723378 Des_Ro_dis=32.196148 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=39.276622 Robot_a=45\n",
      "0.851591669165\n",
      "162410\n",
      "**********************************\n",
      "last_x=855.220934, last_y=893.782972,last_a=315.000000\n",
      "x=863.395088 y=885.608818 True_angle=66.462218 Des_Ro_dis=39.332248 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=21.462218 Robot_a=315\n",
      "0.851234909936\n",
      "162834\n",
      "**********************************\n",
      "last_x=865.728612, last_y=865.983074,last_a=90.000000\n",
      "x=865.728612 y=877.543074 True_angle=56.764479 Des_Ro_dis=40.973669 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=33.235521 Robot_a=90\n",
      "0.850131933445\n",
      "164146\n",
      "**********************************\n",
      "last_x=862.159492, last_y=873.110918,last_a=45.000000\n",
      "x=870.333646 y=881.285072 True_angle=12.754348 Des_Ro_dis=35.076218 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=32.245652 Robot_a=45\n",
      "0.849858934931\n",
      "164471\n",
      "**********************************\n",
      "last_x=851.129805, last_y=893.102991,last_a=45.000000\n",
      "x=859.303959 y=901.277145 True_angle=46.797497 Des_Ro_dis=40.716076 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-1.797497 Robot_a=45\n",
      "0.849572591858\n",
      "164812\n",
      "**********************************\n",
      "last_x=869.239018, last_y=862.846621,last_a=45.000000\n",
      "x=877.413173 y=871.020776 True_angle=7.066633 Des_Ro_dis=36.741805 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=52.066633 Robot_a=45\n",
      "0.84828567482\n",
      "166346\n",
      "**********************************\n",
      "last_x=850.429247, last_y=877.153603,last_a=0.000000\n",
      "x=861.989247 y=877.153603 True_angle=31.008009 Des_Ro_dis=44.348340 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=31.008009 Robot_a=0\n",
      "0.847727562433\n",
      "167012\n",
      "**********************************\n",
      "last_x=891.587429, last_y=850.330551,last_a=90.000000\n",
      "x=891.587429 y=861.890551 True_angle=12.448271 Des_Ro_dis=39.026932 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=77.551729 Robot_a=90\n",
      "0.847513969139\n",
      "167267\n",
      "**********************************\n",
      "last_x=863.759248, last_y=869.318992,last_a=45.000000\n",
      "x=871.933403 y=877.493146 True_angle=6.273564 Des_Ro_dis=35.976275 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=38.726436 Robot_a=45\n",
      "0.846742973871\n",
      "168188\n",
      "**********************************\n",
      "last_x=904.275175, last_y=851.733381,last_a=90.000000\n",
      "x=904.275175 y=863.293381 True_angle=6.643238 Des_Ro_dis=36.954743 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-83.356762 Robot_a=90\n",
      "0.846312162183\n",
      "168703\n",
      "**********************************\n",
      "last_x=869.019369, last_y=872.173996,last_a=45.000000\n",
      "x=877.193524 y=880.348151 True_angle=4.249233 Des_Ro_dis=30.105324 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=40.750767 Robot_a=45\n",
      "0.845252390399\n",
      "169971\n",
      "**********************************\n",
      "last_x=864.096649, last_y=862.606303,last_a=0.000000\n",
      "x=875.656649 y=862.606303 True_angle=56.935814 Des_Ro_dis=44.619361 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=56.935814 Robot_a=0\n",
      "0.843723608811\n",
      "171803\n",
      "**********************************\n",
      "last_x=866.315969, last_y=863.478150,last_a=90.000000\n",
      "x=866.315969 y=875.038150 True_angle=53.459377 Des_Ro_dis=41.925027 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=36.540623 Robot_a=90\n",
      "0.842953603993\n",
      "172727\n",
      "**********************************\n",
      "last_x=848.873120, last_y=909.451496,last_a=0.000000\n",
      "x=860.433120 y=909.451496 True_angle=13.434713 Des_Ro_dis=40.680078 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-13.434713 Robot_a=0\n",
      "0.842722907798\n",
      "173004\n",
      "**********************************\n",
      "last_x=861.578177, last_y=876.470601,last_a=45.000000\n",
      "x=869.752331 y=884.644756 True_angle=18.085350 Des_Ro_dis=33.922043 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=26.914650 Robot_a=45\n",
      "0.842371572868\n",
      "173426\n",
      "**********************************\n",
      "last_x=855.090605, last_y=883.314403,last_a=0.000000\n",
      "x=866.650605 y=883.314403 True_angle=26.580030 Des_Ro_dis=37.290633 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=26.580030 Robot_a=0\n",
      "0.842358255029\n",
      "173442\n",
      "**********************************\n",
      "last_x=855.703603, last_y=874.667107,last_a=45.000000\n",
      "x=863.877758 y=882.841262 True_angle=19.591411 Des_Ro_dis=39.990482 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=25.408589 Robot_a=45\n",
      "0.841690971325\n",
      "174244\n",
      "**********************************\n",
      "last_x=876.644905, last_y=855.993284,last_a=90.000000\n",
      "x=876.644905 y=867.553284 True_angle=35.746281 Des_Ro_dis=39.978117 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=54.253719 Robot_a=90\n",
      "0.841497209903\n",
      "174477\n",
      "**********************************\n",
      "last_x=909.671761, last_y=858.739707,last_a=90.000000\n",
      "x=909.671761 y=870.299707 True_angle=18.037597 Des_Ro_dis=31.235403 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-71.962403 Robot_a=90\n",
      "0.840935307733\n",
      "175153\n",
      "**********************************\n",
      "last_x=861.495158, last_y=871.510712,last_a=90.000000\n",
      "x=861.495158 y=883.070712 True_angle=66.266518 Des_Ro_dis=42.062140 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=23.733482 Robot_a=90\n",
      "0.840350535141\n",
      "175857\n"
     ]
    }
   ],
   "source": [
    "def run2():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Epoche_action_interval\n",
    "#     global AR\n",
    "#     global Cr\n",
    "    global Vec_x_\n",
    "    global Vec_y_\n",
    "    Crash_=False\n",
    "    Arrive_=False\n",
    "    Destination_x_,Destination_y_=Set_destination(900,900)\n",
    "    Current_x_,Current_y_,Current_a_=Random_start()\n",
    "    Epoche_false=0\n",
    "    Vec_x_=[]\n",
    "    Vec_y_=[]\n",
    "#     AR=[]\n",
    "#     Cr=[]\n",
    "    Vec_x_.append(Current_x_)\n",
    "    Vec_y_.append(Current_y_)\n",
    "    while Epoche<100:\n",
    "        True_a_,Le_ri_,Distance_=Robot_desti_(Destination_x_,Destination_y_,Current_x_,Current_y_,Current_a_)\n",
    "        Current_state_number_,Dis_level_,Angle_level_=Output_state_index(True_a_,Le_ri_,Distance_)\n",
    "        Next_action_=Choose_action2(Q_Table2,Current_state_number_,Action_times)\n",
    "        Next_x,Next_y,Next_a,R_,Next_state_number_,Arrive_,Crash_=Next_state_F2(Current_x_,Current_y_,Current_a_,Destination_x_,Destination_y_,Next_action_)\n",
    "#         print('%f  %f'%(Next_x,Next_y))\n",
    "#         AR.append(Arrive_)\n",
    "#         Cr.append(Crash_)\n",
    "        if Crash_==True:\n",
    "            Next_x,Next_y,Next_a=Random_start()\n",
    "#             print('Run Boom!')\n",
    "        else:\n",
    "            if Arrive_==True:\n",
    "                Next_True_a,Next_lr,Next_Dis=Robot_desti_(Destination_x_,Destination_y_,Next_x,Next_y,Next_a)\n",
    "                print('**********************************')\n",
    "                print('last_x=%f, last_y=%f,last_a=%f'%(Current_x_,Current_y_,Current_a_%360))\n",
    "                print('x=%f y=%f True_angle=%f Des_Ro_dis=%f Action=%d S_N=%d'%(Next_x,Next_y,Next_True_a,Next_Dis,Next_action_,Next_state_number_))\n",
    "                print(States2[Next_state_number_])\n",
    "                print('Des_Robot_a=%f Robot_a=%d'%((np.arctan((Next_y-Destination_y_)/(Next_x-Destination_x_)))*180/np.pi,Next_a%360))\n",
    "                print(Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times))\n",
    "                print(Action_times)\n",
    "                Next_x,Next_y,Next_a=Random_start()\n",
    "                Epoche+=1\n",
    "#                 Plot_Move(Vec_x_,Vec_y_,Destination_x_,Destination_y_)\n",
    "            Q_target=R_+Beta*max(Q_Table2[Next_state_number_])\n",
    "            Q_Table2[Current_state_number_][Next_action_]+=Alpha*(Q_target-Q_Table2[Current_state_number_][Next_action_])\n",
    "            Vec_x_.append(Current_x_)\n",
    "            Vec_y_.append(Current_y_)\n",
    "        Current_x_=Next_x\n",
    "        Current_y_=Next_y\n",
    "        Current_a_=Next_a\n",
    "        Action_times+=1\n",
    "    return Q_Table2\n",
    "Q_Table2_final=run2()\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 206,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style>\n",
       "    .dataframe thead tr:only-child th {\n",
       "        text-align: right;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: left;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>Up</th>\n",
       "      <th>Left_45D</th>\n",
       "      <th>Right_45D</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>L60D0/30</th>\n",
       "      <td>213.320457</td>\n",
       "      <td>151.701964</td>\n",
       "      <td>129.363169</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D0/30</th>\n",
       "      <td>-5.000000</td>\n",
       "      <td>-5.122200</td>\n",
       "      <td>-6.430689</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D0/30</th>\n",
       "      <td>-9.999994</td>\n",
       "      <td>-10.747137</td>\n",
       "      <td>-11.638006</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D0/30</th>\n",
       "      <td>-15.000000</td>\n",
       "      <td>-15.125421</td>\n",
       "      <td>-17.466534</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D30/60</th>\n",
       "      <td>159.326608</td>\n",
       "      <td>171.284112</td>\n",
       "      <td>82.292497</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D30/60</th>\n",
       "      <td>-6.000000</td>\n",
       "      <td>-5.000000</td>\n",
       "      <td>-8.019956</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D30/60</th>\n",
       "      <td>-10.999986</td>\n",
       "      <td>-9.999992</td>\n",
       "      <td>-13.237756</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D30/60</th>\n",
       "      <td>-16.000000</td>\n",
       "      <td>-15.000000</td>\n",
       "      <td>-17.873848</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D60/90</th>\n",
       "      <td>84.772829</td>\n",
       "      <td>146.462349</td>\n",
       "      <td>59.256554</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D60/90</th>\n",
       "      <td>-7.581085</td>\n",
       "      <td>-5.566533</td>\n",
       "      <td>-11.116762</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D60/90</th>\n",
       "      <td>-12.279877</td>\n",
       "      <td>-10.230117</td>\n",
       "      <td>-15.619297</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D60/90</th>\n",
       "      <td>-17.546900</td>\n",
       "      <td>-15.525642</td>\n",
       "      <td>-20.427493</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D90/120</th>\n",
       "      <td>52.528448</td>\n",
       "      <td>114.919363</td>\n",
       "      <td>5.628012</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D90/120</th>\n",
       "      <td>-9.527325</td>\n",
       "      <td>-6.538483</td>\n",
       "      <td>-14.262670</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D90/120</th>\n",
       "      <td>-14.721216</td>\n",
       "      <td>-12.139681</td>\n",
       "      <td>-18.117852</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D90/120</th>\n",
       "      <td>-19.469774</td>\n",
       "      <td>-17.449254</td>\n",
       "      <td>-24.528670</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D120/150</th>\n",
       "      <td>11.912901</td>\n",
       "      <td>47.011553</td>\n",
       "      <td>-0.529462</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D120/150</th>\n",
       "      <td>-12.473361</td>\n",
       "      <td>-8.207680</td>\n",
       "      <td>-15.197024</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D120/150</th>\n",
       "      <td>-17.057070</td>\n",
       "      <td>-14.111284</td>\n",
       "      <td>-20.099352</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D120/150</th>\n",
       "      <td>-21.663891</td>\n",
       "      <td>-17.864688</td>\n",
       "      <td>-25.784052</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D150/180</th>\n",
       "      <td>1.871782</td>\n",
       "      <td>23.362959</td>\n",
       "      <td>-1.420308</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D150/180</th>\n",
       "      <td>-15.349406</td>\n",
       "      <td>-10.207282</td>\n",
       "      <td>-13.810385</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D150/180</th>\n",
       "      <td>-19.765422</td>\n",
       "      <td>-15.415245</td>\n",
       "      <td>-18.285339</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D150/180</th>\n",
       "      <td>-25.720888</td>\n",
       "      <td>-20.833575</td>\n",
       "      <td>-23.887567</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D180/210</th>\n",
       "      <td>11.834954</td>\n",
       "      <td>8.848024</td>\n",
       "      <td>36.427543</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D180/210</th>\n",
       "      <td>-14.783985</td>\n",
       "      <td>-14.218496</td>\n",
       "      <td>-9.975879</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D180/210</th>\n",
       "      <td>-20.303374</td>\n",
       "      <td>-18.066777</td>\n",
       "      <td>-16.070217</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D180/210</th>\n",
       "      <td>-25.702109</td>\n",
       "      <td>-24.761627</td>\n",
       "      <td>-20.703397</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D210/240</th>\n",
       "      <td>13.440884</td>\n",
       "      <td>8.288465</td>\n",
       "      <td>41.579304</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D210/240</th>\n",
       "      <td>-11.826760</td>\n",
       "      <td>-15.133065</td>\n",
       "      <td>-8.673158</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D210/240</th>\n",
       "      <td>-16.887781</td>\n",
       "      <td>-20.324011</td>\n",
       "      <td>-13.047413</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D210/240</th>\n",
       "      <td>-22.501644</td>\n",
       "      <td>-25.633562</td>\n",
       "      <td>-18.103847</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D240/270</th>\n",
       "      <td>50.508960</td>\n",
       "      <td>13.451666</td>\n",
       "      <td>114.059388</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D240/270</th>\n",
       "      <td>-9.556176</td>\n",
       "      <td>-13.060233</td>\n",
       "      <td>-6.604208</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D240/270</th>\n",
       "      <td>-14.622838</td>\n",
       "      <td>-19.669806</td>\n",
       "      <td>-11.726406</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D240/270</th>\n",
       "      <td>-19.239239</td>\n",
       "      <td>-24.180677</td>\n",
       "      <td>-16.879544</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D270/300</th>\n",
       "      <td>86.814913</td>\n",
       "      <td>33.498124</td>\n",
       "      <td>134.220311</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D270/300</th>\n",
       "      <td>-7.497702</td>\n",
       "      <td>-11.255171</td>\n",
       "      <td>-5.354615</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D270/300</th>\n",
       "      <td>-12.279711</td>\n",
       "      <td>-15.386777</td>\n",
       "      <td>-10.523652</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D270/300</th>\n",
       "      <td>-17.217300</td>\n",
       "      <td>-20.308006</td>\n",
       "      <td>-15.395176</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D300/330</th>\n",
       "      <td>153.857492</td>\n",
       "      <td>101.462346</td>\n",
       "      <td>176.894064</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D300/330</th>\n",
       "      <td>-6.000000</td>\n",
       "      <td>-8.704633</td>\n",
       "      <td>-5.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D300/330</th>\n",
       "      <td>-10.999991</td>\n",
       "      <td>-13.542769</td>\n",
       "      <td>-9.999993</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D300/330</th>\n",
       "      <td>-16.000000</td>\n",
       "      <td>-18.958320</td>\n",
       "      <td>-15.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D330/360</th>\n",
       "      <td>196.573049</td>\n",
       "      <td>128.055230</td>\n",
       "      <td>150.516465</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D330/360</th>\n",
       "      <td>-4.999999</td>\n",
       "      <td>-7.056798</td>\n",
       "      <td>-5.367274</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D330/360</th>\n",
       "      <td>-9.999989</td>\n",
       "      <td>-11.899893</td>\n",
       "      <td>-10.605341</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D330/360</th>\n",
       "      <td>-15.000000</td>\n",
       "      <td>-16.705549</td>\n",
       "      <td>-15.301899</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "                      Up    Left_45D   Right_45D\n",
       "L60D0/30      213.320457  151.701964  129.363169\n",
       "L180D0/30      -5.000000   -5.122200   -6.430689\n",
       "L250D0/30      -9.999994  -10.747137  -11.638006\n",
       "L300D0/30     -15.000000  -15.125421  -17.466534\n",
       "L60D30/60     159.326608  171.284112   82.292497\n",
       "L180D30/60     -6.000000   -5.000000   -8.019956\n",
       "L250D30/60    -10.999986   -9.999992  -13.237756\n",
       "L300D30/60    -16.000000  -15.000000  -17.873848\n",
       "L60D60/90      84.772829  146.462349   59.256554\n",
       "L180D60/90     -7.581085   -5.566533  -11.116762\n",
       "L250D60/90    -12.279877  -10.230117  -15.619297\n",
       "L300D60/90    -17.546900  -15.525642  -20.427493\n",
       "L60D90/120     52.528448  114.919363    5.628012\n",
       "L180D90/120    -9.527325   -6.538483  -14.262670\n",
       "L250D90/120   -14.721216  -12.139681  -18.117852\n",
       "L300D90/120   -19.469774  -17.449254  -24.528670\n",
       "L60D120/150    11.912901   47.011553   -0.529462\n",
       "L180D120/150  -12.473361   -8.207680  -15.197024\n",
       "L250D120/150  -17.057070  -14.111284  -20.099352\n",
       "L300D120/150  -21.663891  -17.864688  -25.784052\n",
       "L60D150/180     1.871782   23.362959   -1.420308\n",
       "L180D150/180  -15.349406  -10.207282  -13.810385\n",
       "L250D150/180  -19.765422  -15.415245  -18.285339\n",
       "L300D150/180  -25.720888  -20.833575  -23.887567\n",
       "L60D180/210    11.834954    8.848024   36.427543\n",
       "L180D180/210  -14.783985  -14.218496   -9.975879\n",
       "L250D180/210  -20.303374  -18.066777  -16.070217\n",
       "L300D180/210  -25.702109  -24.761627  -20.703397\n",
       "L60D210/240    13.440884    8.288465   41.579304\n",
       "L180D210/240  -11.826760  -15.133065   -8.673158\n",
       "L250D210/240  -16.887781  -20.324011  -13.047413\n",
       "L300D210/240  -22.501644  -25.633562  -18.103847\n",
       "L60D240/270    50.508960   13.451666  114.059388\n",
       "L180D240/270   -9.556176  -13.060233   -6.604208\n",
       "L250D240/270  -14.622838  -19.669806  -11.726406\n",
       "L300D240/270  -19.239239  -24.180677  -16.879544\n",
       "L60D270/300    86.814913   33.498124  134.220311\n",
       "L180D270/300   -7.497702  -11.255171   -5.354615\n",
       "L250D270/300  -12.279711  -15.386777  -10.523652\n",
       "L300D270/300  -17.217300  -20.308006  -15.395176\n",
       "L60D300/330   153.857492  101.462346  176.894064\n",
       "L180D300/330   -6.000000   -8.704633   -5.000000\n",
       "L250D300/330  -10.999991  -13.542769   -9.999993\n",
       "L300D300/330  -16.000000  -18.958320  -15.000000\n",
       "L60D330/360   196.573049  128.055230  150.516465\n",
       "L180D330/360   -4.999999   -7.056798   -5.367274\n",
       "L250D330/360   -9.999989  -11.899893  -10.605341\n",
       "L300D330/360  -15.000000  -16.705549  -15.301899"
      ]
     },
     "execution_count": 206,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Q_Table2_=pd.DataFrame(Q_Table2_final,columns=Actions2,index=States2)\n",
    "Q_Table2_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 207,
   "metadata": {},
   "outputs": [],
   "source": [
    "# print(Cr[-10:-1])\n",
    "# print(Vec_x_[-15:-1])\n",
    "# print(Vec_y_[-10:-1])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
